using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using System.Net.Sockets;
using System.Threading;

// OPEN_ROBOT Class Library Namespace. This project was created so that OPEN_ROBOT users can create their //
// own applications using Visual Studio or C# Express. This class library requires a minimum of .NET 2.0. //
namespace OPEN_ROBOT_TCP_Class_Library
{
    /// <summary>
    /// OPEN_ROBOT TCP-IP Functions Class. Contains all the necessary methods to communicate with OPEN_ROBOT via a MatchPort b/g.
    /// </summary>
    public class OPEN_ROBOT_TCP_Functions
    {
        /// <summary>
        /// Used to determine if SRV1 camera is being used.
        /// </summary>
        private bool isSrv1CamConnected;

        /// <summary>
        /// Declare an instance of the Socket Client.
        /// </summary>
        private Socket m_socClient;

        /// <summary>
        /// Variable to hold Socket Related Errors.
        /// </summary>
        private string socketError;

        /// <summary>
        /// State variable for Socket State.
        /// </summary>
        public bool socketOpen;

        /// <summary>
        /// OPEN_ROBOT_TCP Functions Class Constructor.
        /// </summary>
        public OPEN_ROBOT_TCP_Functions()
        {
            isSrv1CamConnected = false;
            socketOpen = false;
        }

        /// <summary>
        /// Function to open a socket connection with the OPEN_ROBOT's MatchPort.
        /// </summary>
        /// <param name="ipAddress">IP Address of the Robot's MatchPort.</param>
        /// <param name="portNumber">Port number for Serial Port#1 on MatchPort is 10001.</param>
        public void OpenSocketConnection(string ipAddress, int portNumber)
        {
            try
            {
                m_socClient = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
                System.Net.IPAddress ipAdd = System.Net.IPAddress.Parse(ipAddress);
                System.Net.IPEndPoint remoteEP = new System.Net.IPEndPoint(ipAdd, portNumber);
                m_socClient.Connect(remoteEP);

                socketOpen = true;
                if (isSrv1CamConnected) SendSocketData("~", true, '\r');
                else SendSocketData("", true, '\r');
            }
            catch (SocketException se)
            {
                throw se;
            }
        }

        /// <summary>
        /// Function to close the socket connection.
        /// </summary>
        public void CloseSocketConnection()
        {
            try
            {
                m_socClient.Close();
                socketOpen = false;
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to send socket data to OPEN-ROBOT's MatchPort.
        /// </summary>
        /// <param name="socketData">Socket data to send.</param>
        /// <param name="useTerminatingChar">Whether to use a terminating character.</param>
        /// <param name="terminatingChar">Caracter to be used as terminating character.</param>
        public void SendSocketData(string socketData, bool useTerminatingChar, char terminatingChar)
        {
            // Let's check to see if there is data from a previous command residing in the que.
            // If so, go ahead and clear it out.
            if (m_socClient.Available > 0) ClearSocketBuffer(); 

            try
            {
                if (useTerminatingChar == true) socketData += terminatingChar;
                byte[] txBytes = System.Text.Encoding.ASCII.GetBytes(socketData);
                m_socClient.Send(txBytes);
            }
            catch (SocketException se)
            {
                throw se;
            }
        }

        /// <summary>
        /// Function to clear the socket buffer in the event of a read error.
        /// </summary>
        public void ClearSocketBuffer()
        {
            byte[] byteData = new byte[1];

            try
            {
                while (m_socClient.Available > 0)
                {
                    m_socClient.Receive(byteData);
                    Thread.Sleep(0);
                }
            }
            catch (SocketException se)
            {
                throw se;
            }
        }

        /// <summary>
        /// Function to Get JPEG Image from SRV1 Camera.
        /// </summary>
        /// <param name="timeOutMS">Read timeout in milliseconds.</param>
        /// <returns>Returns byte array of camera data if successful, otherwise returns a byte array of lenght 0.</returns>
        public byte[] GetJpegImage(int timeOutMS)
        {
            byte[] errorByte = new byte[1];

            try
            {

                SendSocketData("I", false, Convert.ToChar(0));
                byte[] imageInfo = GetSocketDataBytes(timeOutMS, 10);

                if (imageInfo.Length > 0)
                {
                    int s0, s1, s2, s3, imageSize;
                    s0 = Convert.ToInt32(imageInfo[6]);
                    s1 = Convert.ToInt32(imageInfo[7]);
                    s2 = Convert.ToInt32(imageInfo[8]);
                    s3 = Convert.ToInt32(imageInfo[9]);

                    imageSize = s0 + s1 * 256 + s2 * (int)(Math.Pow(256, 2)) + s3 * (int)(Math.Pow(256, 3));

                    byte[] imageBytes = GetSocketDataBytes(timeOutMS, imageSize);
                    return imageBytes;
                }
                else
                {
                    throw new Exception("Error reading data!");
                }
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to return Raw YUV Image. This function requires a special firmware change to the SRV1 Camera Board!
        /// </summary>
        /// <param name="timeOut"></param>
        /// <param name="numBytes"></param>
        /// <returns></returns>
        public byte[] GetRawImage(int timeOut, int numBytes)
        {
            byte[] errorByte = new byte[1];

            if (socketOpen == false)
            {
                errorByte[0] = 1;
                return errorByte;
            }

            SendSocketData("@", false, Convert.ToChar(0));

            byte[] imageData = GetSocketDataBytes(timeOut, numBytes);

            if (imageData.Length > 0)
            {
                return imageData;
            }
            return errorByte;
        }

        /// <summary>
        /// Function to set the JPEG Quality for the SRV1 Camera.
        /// </summary>
        /// <param name="qualityLevel">Quality level of SRV1 JPEG images.</param>
        /// <param name="timeOutMS">Read time out in milliseconds.</param>
        public void SetJpegQuality(int qualityLevel, int timeOutMS)
        {
            try
            {
                if (qualityLevel < 1) qualityLevel = 1;
                if (qualityLevel > 8) qualityLevel = 8;

                SendSocketData("q" + qualityLevel.ToString(), false, Convert.ToChar(0));
                byte[] byteData = GetSocketDataBytes(timeOutMS, '\n');

                if (byteData.Length < 13) throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }     
        }

        /// <summary>
        /// Function to Set the SRV1 Camera Resolution.
        /// </summary>
        /// <param name="cameraResolution">Camera resolution.</param>
        /// <param name="timeOutMS">Read time out in milliseconds.</param>
        public void SetSRV1CameraResolution(string cameraResolution, int timeOutMS)
        {
            string strData = "";

            try
            {
                // cameraResolution = 160x128, 320x256, 640x512, 1280x1024
                switch (cameraResolution)
                {
                    case "160x128":
                        strData = "a";
                        break;
                    case "320x256":
                        strData = "b";
                        break;
                    case "640x512":
                        strData = "c";
                        break;
                    case "1280x1024":
                        strData = "A";
                        break;
                    default:
                        strData = "a";
                        break;
                }

                SendSocketData(strData, false, Convert.ToChar(0));
                byte[] byteData = GetSocketDataBytes(timeOutMS, 2);
                if (byteData.Length == 0) throw new Exception("Error reading data!");
                if (Convert.ToChar(byteData[0]) != '#' || Convert.ToChar(byteData[1]) != strData[0]) throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to read incoming socket data and return as byte array.
        /// </summary>
        /// <param name="timeOutMS">Read time out in milliseconds.</param>
        /// <param name="terminatingChar">Character that determines end of command sequence.</param>
        /// <returns>Returns byte array with socket data if success, otherwise returns byte array of length 0.</returns>
        public byte[] GetSocketDataBytes(int timeOutMS, char terminatingChar)
        {
            byte[] rxBytes = new byte[0];
            byte[] errorByte = new byte[0];
            byte[] dataByte = new byte[1];
            int iRX, numBytesRx;

            numBytesRx = 0;

            int startTick = System.Environment.TickCount;

            try
            {
                while (System.Environment.TickCount < startTick + timeOutMS)
                {
                    if (m_socClient.Available > 0)
                    {
                        iRX = m_socClient.Receive(dataByte);
                        byte[] tmpBytes = new byte[rxBytes.Length + 1];
                        Array.Copy(rxBytes, tmpBytes, Math.Min(rxBytes.Length, tmpBytes.Length));
                        rxBytes = new byte[tmpBytes.Length];
                        rxBytes = tmpBytes;
                        rxBytes[numBytesRx] = dataByte[0];
                        if (dataByte[0] == terminatingChar) return rxBytes;
                        numBytesRx++;
                        startTick = System.Environment.TickCount;
                    }
                    Thread.Sleep(0);
                }
            }
            catch (SocketException se)
            {
                socketError = se.ToString();
                //Clear_Socket_Buffer();
                ClearSocketBuffer();
                return errorByte;
            }
            return errorByte;
        }

        /// <summary>
        /// Function to read incoming socket data and return as byte array.
        /// </summary>
        /// <param name="timeOutMS">Read time out in milliseconds.</param>
        /// <param name="numBytes">Number of bytes to be read from buffer.</param>
        /// <returns>Returns byte array with socket data if success, otherwise returns byte array of length 0.</returns>
        public byte[] GetSocketDataBytes(int timeOutMS, int numBytes)
        {
            byte[] rxBytes = new byte[numBytes];
            byte[] errorByte = new byte[0];
            byte[] tmpByte = new byte[1];
            int iRX, numBytesRx;

            numBytesRx = 0;

            try
            {
                m_socClient.ReceiveTimeout = timeOutMS;
                while (numBytesRx < numBytes)
                {
                    iRX = m_socClient.Receive(tmpByte);
                    if (iRX != 0)
                    {
                        rxBytes[numBytesRx] = tmpByte[0];
                        numBytesRx++;
                        if (numBytesRx >= numBytes) return rxBytes;
                    }
                    Thread.Sleep(0);
                }
            }
            catch (SocketException se)
            {
                socketError = se.ToString();
                // Clear Socket Buffer().
                ClearSocketBuffer();
                return errorByte;
            }
            return errorByte;
        }

        /// <summary>
        /// Function to move robot using position control.
        /// </summary>
        /// <param name="leftWheel">Number of left wheel encoder ticks. Valid range is -500 to 500.</param>
        /// <param name="rightWheel">Number of right wheel encoder ticks. Valid range is -500 to 500.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void MoveToPosition(int leftWheel, int rightWheel, int timeOut, int numRetries)
        {
            try
            {
                string strCommand = leftWheel.ToString() + "," + rightWheel.ToString();
                string robotData = SendOpenRobotData("MP" + strCommand, true, true, 'm', timeOut, numRetries);
                if (robotData != "ma") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }
        
        /// <summary>
        /// Function to set open loop wheel velocity.
        /// </summary>
        /// <param name="leftWheelVelocity">Left wheel velocity. Valid range is -60 to 60.</param>
        /// <param name="rightWheelVelocity">Right wheel velocity. Valid range is -60 to 60.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetOpenLoopVelocity(int leftWheelVelocity, int rightWheelVelocity, int timeOut, int numRetries)
        {
            try
            {
                string strCommand = leftWheelVelocity.ToString() + "," + rightWheelVelocity.ToString();
                string robotData = SendOpenRobotData("MO" + strCommand, true, true, 'm', timeOut, numRetries);
                if (robotData != "ma") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set the position pid gains.
        /// </summary>
        /// <param name="Kp">Proportional gain. Valid range is 0 to 255.</param>
        /// <param name="Ki">Integral gain. Valid range is 0 to 255.</param>
        /// <param name="Kd">Derivative gain. Valid range is 0 to 255.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetPositionGainsPID(int Kp, int Ki, int Kd, int timeOut, int numRetries)
        {
            if (Kp > 255) Kp = 255;
            else if (Ki > 255) Ki = 255;
            else if (Kd > 255) Kd = 255;
            else if (Kp < 0) Kp = 0;
            else if (Ki < 0) Ki = 0;
            else if (Kd < 0) Kd = 0;

            try
            {
                string robotData = SendOpenRobotData("PP" + Kp.ToString(), true, true, 'p', timeOut, numRetries);
                if (robotData != "p") throw new Exception("Error reading data!");

                robotData = SendOpenRobotData("PI" + Ki.ToString(), true, true, 'p', timeOut, numRetries);
                if (robotData != "p") throw new Exception("Error reading data!");

                robotData = SendOpenRobotData("PD" + Kd.ToString(), true, true, 'p', timeOut, numRetries);
                if (robotData != "p") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set the PWM Motor Offset.
        /// </summary>
        /// <param name="pwm_offset">PWM Motor Offset Parameter.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="leftWheel">Set to true to adjust left wheel pwm, otherwise false for the right wheel.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetPwmOffset(int pwm_offset, bool leftWheel, int timeOut, int numRetries)
        {
            string robotData;

            try
            {
                if (leftWheel) robotData = SendOpenRobotData("@L" + pwm_offset.ToString(), true, true, '@', timeOut, numRetries);
                else robotData = SendOpenRobotData("@R" + pwm_offset.ToString(), true, true, '@', timeOut, numRetries);
                if (robotData != "@") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set Closed loop wheel velocity.
        /// </summary>
        /// <param name="leftWheelVelocity">Left wheel velocity. Valid range is -60 to 60 tenths of an inch per second.</param>
        /// <param name="rightWheelVelocity">Right wheel velocity. Valid range is -60 to 60 tenths of an inch per second.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetClosedLoopVelocity(int leftWheelVelocity, int rightWheelVelocity, int timeOut, int numRetries)
        {
            string robotData;

            try
            {
                string strCommand = leftWheelVelocity.ToString() + "," + rightWheelVelocity.ToString();
                robotData = SendOpenRobotData("MV" + strCommand, true, true, 'm', timeOut, numRetries);
                if (robotData != "ma") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set digital pin high.
        /// </summary>
        /// <param name="pinNum">Digital pin number. Valid range is 0 to 7.</param>
        /// <param name="pinState">State of pin: true = high and false = low.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetDigitalPinState(int pinNum, bool pinState, int timeOut, int numRetries)
        {
            if (pinNum > 7) pinNum = 7;
            else if (pinNum < 0) pinNum = 0;

            string robotData;

            try
            {
                if (pinState)
                {
                    robotData = SendOpenRobotData("K" + pinNum.ToString(), true, true, 'k', timeOut, numRetries);
                    if (robotData != "k") throw new Exception("Error reading data!");
                }
                else
                {
                    robotData = SendOpenRobotData("J" + pinNum.ToString(), true, true, 'j', timeOut, numRetries);
                    if (robotData != "j") throw new Exception("Error reading data!");
                }
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set light reflex level.
        /// </summary>
        /// <param name="reflexLevel">Light reflex level. Valid range is 0 to 255.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetLightReflexLevel(int reflexLevel, int timeOut, int numRetries)
        {
            if (reflexLevel > 255) reflexLevel = 255;
            if (reflexLevel < 0) reflexLevel = 0;

            try
            {
                string robotData = SendOpenRobotData("N" + reflexLevel.ToString(), true, true, 'n', timeOut, numRetries);
                if (robotData != "n") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set sensor reflexes.
        /// </summary>
        /// <param name="strBitString">Sensor reflexes bit string. Valid range is 8-bit string of 1's and 0's.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetSensorReflexes(string strBitString, int timeOut, int numRetries)
        {
            if (strBitString.Length != 8) strBitString = "00000000";

            try
            {
                for (int i = 0; i < 8; i++)
                {
                    if (strBitString[i] != '1' && strBitString[i] != '0') strBitString = "00000000";
                }

                //Convert to byte.
                byte byteVal = System.Convert.ToByte(strBitString, 2);

                string robotData = SendOpenRobotData("I" + byteVal.ToString(), true, true, 'i', timeOut, numRetries);
                if (robotData != "i") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set velocity pid gains.
        /// </summary>
        /// <param name="Kp">Proportional gain. Valid range is 0 to 255.</param>
        /// <param name="Ki">Integral gain. Valid range is 0 to 255.</param>
        /// <param name="Kd">Derivative gain. Valid range is 0 to 255.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetVelocityGainsPID(int Kp, int Ki, int Kd, int timeOut, int numRetries)
        {
            if (Kp > 255) Kp = 255;
            else if (Ki > 255) Ki = 255;
            else if (Kd > 255) Kd = 255;
            else if (Kp < 0) Kp = 0;
            else if (Ki < 0) Ki = 0;
            else if (Kd < 0) Kd = 0;
            
            try
            {
                string robotData = SendOpenRobotData("QP" + Kp.ToString(), true, true, 'q', timeOut, numRetries);
                if (robotData != "q") throw new Exception("Error reading data!");

                robotData = SendOpenRobotData("QI" + Ki.ToString(), true, true, 'q', timeOut, numRetries);
                if (robotData != "q") throw new Exception("Error reading data!");

                robotData = SendOpenRobotData("QD" + Kd.ToString(), true, true, 'q', timeOut, numRetries);
                if (robotData != "q") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to stop searching for in-range RFID tags.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void StopRfidTagSearch(int timeOut, int numRetries)
        {
            try
            {
                //Write the Stop Searching for RFID tag command.
                string robotData = SendOpenRobotData("H", true, true, 'h', timeOut, numRetries);
                if (robotData != "h") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to make robot enter wander mode and drive to avoid obstacles.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void StartWanderMode(int timeOut, int numRetries)
        {
            try
            {
                //Write the wander command.
                string robotData = SendOpenRobotData("U", true, true, 'u', timeOut, numRetries);
                if (robotData != "u") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to stop the agent immediately.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void StopRobot(int timeOut, int numRetries)
        {
            try
            {
                //Write the halt command.
                string robotData = SendOpenRobotData("H", true, true, 'h', timeOut, numRetries);
                if (robotData != "h") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to Send Data Out the Socket Connection with the MatchPort.
        /// </summary>
        /// <param name="strData">Socket data.</param>
        /// <param name="bSendCR">Command character.</param>
        /// <param name="bUseStartChar">Determines whether to use a start character.</param>
        /// <param name="chrStartChar">First returned character.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of retries before failing.</param>
        /// <returns>String containing received robot data.</returns>
        private string SendOpenRobotData(string strData, bool bSendCR, bool bUseStartChar, char chrStartChar, int timeOut, int numRetries)
        {
            string robotData;
            int retryCount = 0;
            
            robotData = string.Empty;

            try
            {
            SendDataAgain:

                // If the SRV1-Camera Board is connected then we need to send '~' so that the modified SRV1 firmware
                // can recognize an OPEN-ROBOT command.
                if (isSrv1CamConnected) SendSocketData("~" + strData, bSendCR, '\r');
                else SendSocketData(strData, bSendCR, '\r');
                                
                byte[] dataBytes = GetSocketDataBytes(timeOut, '\r');
                if (dataBytes.Length == 0)
                {
                    if (retryCount < numRetries)
                    {
                        retryCount++;
                        goto SendDataAgain;
                    }
                    throw new Exception("Error reading data!");
                }
                if (dataBytes[0] != chrStartChar)
                {
                    throw new Exception("Error reading data!");
                }
                robotData = string.Empty;
                for (int i = 0; i < dataBytes.Length; i++)
                {
                    robotData += Convert.ToChar(dataBytes[i]);
                }
                
                // Strip off the "\r" char.
                string[] strTmp = robotData.Split('\r');
                robotData = strTmp[0];                
            }
            catch (Exception ex)
            {
                throw ex;
            }
            
            return robotData;
        }

        /// <summary>
        /// Function to Extract Comma Delimited Data from a String.
        /// </summary>
        /// <param name="intOffset">Where to begin parsing data.</param>
        /// <param name="strData">String of data to extract comma delimited data.</param>
        /// <returns>String array of parsed data.</returns>
        private string[] ExtractCommaDelimitedData(int intOffset, string strData)
        {
            string[] strArray = new string[1];
            int comma_index, i, numCommas;
            char[] rs232_chr;

            numCommas = 0;
            //Convert string to char array.
            rs232_chr = strData.ToCharArray();

            try
            {
                //First find out how many commas exist.
                for (i = intOffset; i < rs232_chr.Length; i++)
                {
                    if (rs232_chr[i] == ',') numCommas += 1;
                }

                //Resize strArray.
                strArray = new string[numCommas + 1];
            }
            catch (Exception ex)
            {
                throw ex;
            }

            try
            {
                //Parse out the individual pieces of data and return a string array.
                comma_index = 0;
                for (i = intOffset; i < rs232_chr.Length; i++)
                {
                    if (rs232_chr[i] == ',') comma_index += 1;
                    else strArray[comma_index] += rs232_chr[i];
                }
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return strArray;
        }

        /// <summary>
        /// Function to find in-range RFID tags. Robot will search for in-range tags and stop when a tag is located.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void FindRfidTag(int timeOut, int numRetries)
        {
            try
            {
                //Write the find RFID tag command.
                string robotData = SendOpenRobotData("Z", true, true, 'z', timeOut, numRetries);
                if (robotData != "z") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to convert a hex string to ascii string.
        /// </summary>
        /// <param name="strHex">String of hex data.</param>
        /// <returns>String array. First position in array will contain OK if successful or error. Second position contains converted ASCII data.</returns>
        private string ConvertHexStringToASCIIString(string strHex)
        {
            string strUTF7 = "";
            string strASCII;
                      
            try
            {
                //First make sure that strHex is of length 8 otherwise pad with zeros.
                if (strHex.Length < 8)
                {
                    string strTmpHex = "";
                    int numZeros = 8 - strHex.Length;
                    for (int i = 0; i < numZeros; i++) strTmpHex += "0";
                    for (int i = 0; i < strHex.Length; i++) strTmpHex += strHex[i];
                    strHex = strTmpHex;
                }

                for (int i = 0; i < 7; i = i + 2)
                {
                    byte byteVal = byte.Parse(strHex[i].ToString() + strHex[i + 1].ToString(), System.Globalization.NumberStyles.HexNumber);
                    char utf7Char = (char)byteVal;
                    strUTF7 += utf7Char;
                }
                strASCII = strUTF7;
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return strASCII;
        }

        /// <summary>
        /// Function to get the Analog to Digital Sensor Readings.
        /// </summary>
        /// <param name="timeOut">Read timeout in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>byte[] array containg sensor readings: {battery, FCIR, FLIR, FRIR, RLIR, RRIR, FRL, FLL}.</returns>
        public byte[] GetAnalogToDigitalSensorReadings(int timeOut, int numRetries)
        {
            byte[] adReadings = new byte[8];

            try
            {
                TRY_AGAIN:

                string robotData = SendOpenRobotData("A", true, true, 'a', timeOut, numRetries);

                string[] strReadings = ExtractCommaDelimitedData(1, robotData);
                if (strReadings.Length < 8)
                {
                    if (numRetries > 0) goto TRY_AGAIN;
                    else
                    {
                        return adReadings;
                    }
                }

                // Check each returned value to be sure that it is a valid reading.
                for (int i = 0; i < 8; i++)
                {
                    char[] adChars = strReadings[i].ToCharArray();
                    bool foundDigit = false;
                    for (int j = 0; j < adChars.Length; j++)
                    {
                        if (char.IsNumber(adChars[j]))
                        {
                            foundDigit = true;
                            break;
                        }                        
                    }
                    if (!foundDigit) strReadings[i] = "0";
                }

                // Update the AD Readings.
                adReadings[0] = Convert.ToByte(strReadings[0]);
                adReadings[1] = Convert.ToByte(strReadings[1]);
                adReadings[2] = Convert.ToByte(strReadings[2]);
                adReadings[3] = Convert.ToByte(strReadings[3]);
                adReadings[4] = Convert.ToByte(strReadings[4]);
                adReadings[5] = Convert.ToByte(strReadings[5]);
                adReadings[6] = Convert.ToByte(strReadings[6]);
                adReadings[7] = Convert.ToByte(strReadings[7]);
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return adReadings;
        }

        /// <summary>
        /// Function to get digital pin's state.
        /// </summary>
        /// <param name="intPinNum">Pin number. Valid range is 0 to 7.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>Pin State as boolean.</returns>
        public bool GetDigitalPinState(int intPinNum, int timeOut, int numRetries)
        {
            bool pinState;

            try
            {
                string robotData = SendOpenRobotData("L" + intPinNum.ToString(), true, true, 'l', timeOut, numRetries);
                
                string strTmp = robotData;

                if (strTmp[1] == '1') pinState = true;
                else pinState = false;
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return pinState;
        }

        /// <summary>
        /// Function to refresh the wheel encoder readings.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>int[] array containg left and right encoder readings: {left encoder reading, right encoder reading}.</returns>
        public int[] GetWheelEncoderReadings(int timeOut, int numRetries)
        {
            int[] encoderReadings = new int[2];

            try
            {
                string robotData = SendOpenRobotData("E", true, true, 'e', timeOut, numRetries);

                string[] strReadings = ExtractCommaDelimitedData(1, robotData);

                if (strReadings.Length < 2) throw new Exception("Error reading data!");

                encoderReadings[0] = Convert.ToInt32(strReadings[0]);
                encoderReadings[1] = Convert.ToInt32(strReadings[1]);
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return encoderReadings;
        }

        /// <summary>
        /// Function to get firmware version for the PIC18F4520.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>String containing firmware version or empty string for an error.</returns>
        public string GetFirmwareVersion(int timeOut, int numRetries)
        {
            string robotData;

            try
            {
                robotData = SendOpenRobotData("!", true, true, '!', timeOut, numRetries);
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return robotData;
        }

        /// <summary>
        /// Function to refresh move complete.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>True if move complete or false if robot is still in motion.</returns>
        public bool IsMoveComplete(int timeOut, int numRetries)
        {
            bool moveComplete;

            try
            {
                string robotData = SendOpenRobotData("D", true, true, 'd', timeOut, numRetries);

                if (robotData == "da") moveComplete = true;
                else moveComplete = false;
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return moveComplete;
        }

        /// <summary>
        /// Function to get the position PID gains.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>Byte[] array containing position PID gains: {Kp, Ki, Kd}.</returns>
        public byte[] GetPositionGainsPID(int timeOut, int numRetries)
        {
            byte[] pidGains = new byte[3];

            try
            {
                string robotData = SendOpenRobotData("GP", true, true, 'g', timeOut, numRetries);

                string[] strReadings = ExtractCommaDelimitedData(2, robotData);

                if (strReadings.Length < 3) throw new Exception("Error reading data!");

                pidGains[0] = Convert.ToByte(strReadings[0]);
                pidGains[1] = Convert.ToByte(strReadings[1]);
                pidGains[2] = Convert.ToByte(strReadings[2]);
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return pidGains;
        }

        /// <summary>
        /// Function to refresh the Left and Right PWM Motor Offset Parameters.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>Byte[] array containg PWM offsets: {left PWM offset, right PWM offset}.</returns>
        public byte[] GetPwmOffsets(int timeOut, int numRetries)
        {
            byte[] pwmOffsets = new byte[2];

            try
            {
                string robotData = SendOpenRobotData("@G", true, true, '@', timeOut, numRetries);

                string[] strReadings = ExtractCommaDelimitedData(1, robotData);

                if (strReadings.Length < 2) throw new Exception("Error reading data!");
                
                pwmOffsets[0] = Convert.ToByte(strReadings[0]);
                pwmOffsets[1] = Convert.ToByte(strReadings[1]);
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return pwmOffsets;
        }

        /// <summary>
        /// Function to refresh ad/light sensor reflex level.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>Byte[] array containg relfex levels: {AD reflex level, Light reflex level}.</returns>
        public byte[] GetReflexLevels(int timeOut, int numRetries)
        {
            byte[] reflexLevels = new byte[2];

            try
            {
                string robotData = SendOpenRobotData("F", true, true, 'f', timeOut, numRetries);

                string[] strReadings = ExtractCommaDelimitedData(1, robotData);

                if (strReadings.Length < 2) throw new Exception("Error reading data!");
                
                reflexLevels[0] = Convert.ToByte(strReadings[0]);
                reflexLevels[1] = Convert.ToByte(strReadings[1]);
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return reflexLevels;
        }

        /// <summary>
        /// Function to dtermine if reflex has been triggered.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>True if reflex has been triggered, otherwise false.</returns>
        public bool IsReflexTriggered(int timeOut, int numRetries)
        {
            bool reflexTriggered;

            try
            {
                string robotData = SendOpenRobotData("B", true, true, 'b', timeOut, numRetries);

                if (robotData == "bb") reflexTriggered = true;
                else reflexTriggered = false;
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return reflexTriggered;
        }

        /// <summary>
        /// Function to refresh the velocity pid gains.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>Byte[] array containing velocity PID gains: {Kp, Ki, Kd}.</returns>
        public byte[] GetVelocityGainsPID(int timeOut, int numRetries)
        {
            byte[] pidGains = new byte[3];

            try
            {
                string robotData = SendOpenRobotData("GV", true, true, 'g', timeOut, numRetries);

                string[] strReadings = ExtractCommaDelimitedData(2, robotData);

                if (strReadings.Length < 3) throw new Exception("Error reading data!");
                
                pidGains[0] = Convert.ToByte(strReadings[0]);
                pidGains[1] = Convert.ToByte(strReadings[1]);
                pidGains[2] = Convert.ToByte(strReadings[2]);
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return pidGains;
        }

        /// <summary>
        /// Function to get raw RFID reading
        /// </summary>
        /// <returns>String containg raw RFID data.</returns>
        private string GetRawRfidReading(int timeOut, int numRetries)
        {
            string rfidReading = string.Empty;

            try
            {
                string robotData = SendOpenRobotData("T", true, true, 't', timeOut, numRetries);

                string strTmp = robotData;

                //Check to see if a tag was found.
                if (strTmp[1] == 'n') return "NO_TAG";

                //Check for missing data.
                if (strTmp.Length != 9 && strTmp.Length != 5) throw new Exception("Error reading data!");

                for (int i = 1; i < strTmp.Length; i++) rfidReading += strTmp[i];
            }
            catch (Exception ex)
            {
                return ex.Message;
            }
            return rfidReading;
        }

        /// <summary>
        /// Function to get RFID Reading as ASCII string.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>RFID reading as ASCII string.</returns>
        public string GetAsciiRfidReading(int timeOut, int numRetries)
        {
            string asciiReading = string.Empty;

            try
            {
                string strRawData = GetRawRfidReading(timeOut, numRetries);
                if (strRawData == "NO_TAG") return strRawData;

                //Try to Convert RFID reading.
                if (strRawData.Length == 8)
                {
                    asciiReading = ConvertHexStringToASCIIString(strRawData);
                }
                else if (strRawData.Length == 4)
                {
                    asciiReading = strRawData;
                }
            }
            catch (Exception ex) 
            { 
                throw ex;
            }
            return asciiReading;
        }

        /// <summary>
        /// Function to get RFID Reading as Hex string.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>RFID reading as Hex string.</returns>
        public string GetHexRfidReading(int timeOut, int numRetries)
        {
            string hexReading = string.Empty;

            try
            {
                string strRawData = GetRawRfidReading(timeOut, numRetries);
                if (strRawData == "NO_TAG") return strRawData;

                //Try to Convert RFID reading.
                if (strRawData.Length == 8)
                {
                    hexReading = strRawData;
                }
                else if (strRawData.Length == 4)
                {
                    //First convert to hex string.
                    char[] chrRFIDArray = strRawData.ToCharArray();
                    for (int i = 0; i < chrRFIDArray.Length; i++)
                    {
                        int intHex = Convert.ToByte(chrRFIDArray[i]);
                        hexReading += intHex.ToString("X");
                    }
                }
            }
            catch (Exception ex)
            {
                throw ex;
            }
            return hexReading;
        }

        /// <summary>
        /// Function to Update RFID Tag Found. Used in conjunction with Find_RFID_Tag function. Checks to see if a tag has been located.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>True if tag found, otherwise false.</returns>
        public bool IsRfidTagFound(int timeOut, int numRetries)
        {
            bool tagFound;

            try
            {
                string robotData = SendOpenRobotData("z", true, true, 'z', timeOut, numRetries);

                if (robotData == "za") tagFound = true;
                else tagFound = false;
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return tagFound;
        }

        /// <summary>
        /// Function to get the wheel velocity readings.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        /// <returns>Int[] array containing wheel velocities: {left wheel velocity, right wheel velocity}.</returns>
        public int[] GetWheelVelocities(int timeOut, int numRetries)
        {
            int[] wheelVelocities = new int[2];

            try
            {
                string robotData = SendOpenRobotData("V", true, true, 'v', timeOut, numRetries);

                string[] strReadings = ExtractCommaDelimitedData(1, robotData);

                if (strReadings.Length < 2) throw new Exception("Error reading data!");
                
                wheelVelocities[0] = Convert.ToInt32(strReadings[0]);
                wheelVelocities[1] = Convert.ToInt32(strReadings[1]);
            }
            catch (Exception ex)
            {
                throw ex;
            }

            return wheelVelocities;
        }

        /// <summary>
        /// Function to reset the RFID module.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void ResetRfidModule(int timeOut, int numRetries)
        {
            try
            {
                string robotData = SendOpenRobotData("X", true, true, 'x', timeOut, numRetries);
                if (robotData != "x") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to reset sensor reflex trigger.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void ResetSensorReflexTrigger(int timeOut, int numRetries)
        {
            try
            {
                string robotData = SendOpenRobotData("R", true, true, 'r', timeOut, numRetries);
                if (robotData != "r") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to save OPEN-ROBOT parameters to EEPROM.
        /// </summary>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SaveParametersToEEPROM(int timeOut, int numRetries)
        {
            try
            {
                string robotData = SendOpenRobotData("Y", true, true, 'y', timeOut, numRetries);
                if (robotData != "y") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set the Analog to Digital reflex level.
        /// </summary>
        /// <param name="reflexLevel">AD reflex level. Valid range is 0 to 255.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void SetAnalogToDigitalReflexLevel(int reflexLevel, int timeOut, int numRetries)
        {
            if (reflexLevel > 255) reflexLevel = 255;
            if (reflexLevel < 0) reflexLevel = 0;

            try
            {
                string robotData = SendOpenRobotData("C" + reflexLevel.ToString(), true, true, 'c', timeOut, numRetries);
                if (robotData != "c") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to write RFID data as ASCII characters.
        /// </summary>
        /// <param name="dataRFID">RFID ASCII data. Valid range is 4-characters.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void WriteRfidDataASCII(string dataRFID, int timeOut, int numRetries)
        {
            try
            {
                //Write the RFID data.
                string robotData = SendOpenRobotData("W" + dataRFID, true, true, 'w', timeOut, numRetries);
                if (robotData != "wa") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to write RFID data as a Hex string.
        /// </summary>
        /// <param name="strdataRFID">RFID hex data. Valid range is 0 to FFFFFFFF.</param>
        /// <param name="timeOut">Read time out in milliseconds.</param>
        /// <param name="numRetries">Number of times to resend if no acknowledge from OPEN-ROBOT board.</param>
        public void WriteRfidDataHex(string strdataRFID, int timeOut, int numRetries)
        {
            try
            {
                //First convert hex to an ASCII string.
                string strASCII = ConvertHexStringToASCIIString(strdataRFID);
                
                //Write the RFID data.
                string robotData = SendOpenRobotData("W" + strASCII, true, true, 'w', timeOut, numRetries);
                if (robotData != "wa") throw new Exception("Error reading data!");
            }
            catch (Exception ex)
            {
                throw ex;
            }
        }

        /// <summary>
        /// Function to set SRV1 Camera connection status.
        /// </summary>
        /// <param name="isSRV1CameraConnected">If SRV1 Camera is connected to OPEN-ROBOT set to TRUE, else default is False.</param>
        public void SetSRV1CameraConnectionStatus(bool isSRV1CameraConnected)
        {
            isSrv1CamConnected = isSRV1CameraConnected;
        }
    }
}
